/* main.c - eMIOS OPWM example */
/* Description:  eMIOS example using Modulus Counter and OPWM modes */
/* Rev 1.0 Sept 9 2004 S.Mihalik */
/* Rev 1.1 April 13 2006 S.M.- corrected GPRE to be div by 12 instead of 13*/
/* Rev 1.2 June 26 1006 S.M. - updated comments & made i volatile uint32_t */
/* Rev 1.3 July 19 2007 SM- Changes for MPC551x, 50 MHz sysclk, Mod Ctr data value*/
/* Rev 1.4 Aug 10 2007 SM - Changed to use sysclk of 64 MHz */
/* Rev 1.5 Jun 04 2008 SM - initSysclk changed for MPC5633M support */
/* Rev 1.6 May 22 2009 SM - modified for MPC56xxB/S */
/* Rev 1.7 Jun 24 2008 SM - simplified code */
/* Rev 1.8 Mar 14 2010 SM - modified initModesAndClock, updated header file */
/* Copyright Freescale Semiconductor, Inc. 2007, 2009 All rights reserved. */

#include "MPC5604B_M27V.h" /* Use proper include file */

	vuint32_t i = 0;                      /* Dummy idle counter */
	int velocity = 40;    	// 40 units  10 % ancho de pulso 
	int direction = 600;	//  600 90 grados pocision del servo
/*Equivalencia de unidades para el ancho de pulso 1= 2,5uS,600 = 1,5 mS
*/
   	

void initModesAndClock(void) {
  ME.MER.R = 0x0000001D;          /* Enable DRUN, RUN0, SAFE, RESET modes */
                                  /* Initialize PLL before turning it on: */

/* Use 1 of the next 2 lines depending on crystal frequency: */
  CGM.FMPLL_CR.R = 0x02400100;    /* 8 MHz xtal: Set PLL0 to 64 MHz */   
/*CGM.FMPLL_CR.R = 0x12400100;*/  /* 40 MHz xtal: Set PLL0 to 64 MHz */   
  ME.RUN[0].R = 0x001F0074;       /* RUN0 cfg: 16MHzIRCON,OSC0ON,PLL0ON,syclk=PLL */
  ME.RUNPC[1].R = 0x00000010; 	  /* Peri. Cfg. 1 settings: only run in RUN0 mode */
  ME.PCTL[68].R = 0x01;           /* MPC56xxB/S SIUL:  select ME.RUNPC[0] */	
  ME.PCTL[72].R = 0x01;           /* MPC56xxB/S EMIOS 0:  select ME.RUNPC[0] */	
                                  /* Mode Transition to enter RUN0 mode: */
  ME.MCTL.R = 0x40005AF0;         /* Enter RUN0 Mode & Key */
  ME.MCTL.R = 0x4000A50F;         /* Enter RUN0 Mode & Inverted Key */  
  while (ME.GS.B.S_MTRANS) {}     /* Wait for mode transition to complete */    
                                  /* Note: could wait here using timer and/or I_TC IRQ */
  while(ME.GS.B.S_CURRENTMODE != 4) {} /* Verify RUN0 is the current mode */
}

void initPeriClkGen(void) {
  CGM.SC_DC[2].R = 0x80;          /* MPC56xxB: Enable peri set 3 sysclk divided by 1 */
  								/*System Clock Driver Config Driver enable	*/
}

void disableWatchdog(void) {
  SWT.SR.R = 0x0000c520;     /* Write keys to clear soft lock bit */
  SWT.SR.R = 0x0000d928; 
  SWT.CR.R = 0x8000010A;     /* Clear watchdog enable (WEN) */
}        


void  vfnInit_Emios_Input_Pad(uint8_t u8PcrVal)
{
	SIU.PCR[u8PcrVal].R = 0x0500;      	/* Assign PCR_EMIOS_0_channel as an input emios pad */
}
 /* set the inputs and outputs */
void initCONFIG_output(void) {

	SIU.PCR[64].R = 0x100;
	SIU.PCR[65].R = 0x100;
	SIU.PCR[66].R = 0x100;
	SIU.PCR[67].R = 0x100;

	SIU.PCR[78].R = 0x200; /* set PE[0] as output */
	SIU.PCR[79].R = 0x200; /* set PE[1] as output */
	
	SIU.PCR[1].R = 0x0500; /* Assign PCR_EMIOS_0_channel as an input emios pad */
}

void initFLASHING_led(void) { 

SIU.GPDO[78].R = 1;
SIU.GPDO[79].R = 1;


}




void initEMIOS_0(void) {
//GPREN Global Prescaler Enable
//GTBE Global Time Base Enable
//FRZ  Freeze
  /*Default 0.4 MHz */
  EMIOS_0.MCR.B.GPRE= 159;   /* Divide 64 MHz sysclk by 159+1 = 160 for 0,4 MHz eMIOS clk*/
  EMIOS_0.MCR.B.GPREN = 1;	/* Enable eMIOS clock */
  EMIOS_0.MCR.B.GTBE = 1;   /* Enable global time base */
  EMIOS_0.MCR.B.FRZ = 1;    /* Enable stopping channels when in debug mode */
}

//**************************************************************************************************
//**************************************************************************************************
//**************************************************************************************************
//Divisores de frecuencia para la camara CK

void initEMIOS_0ch0(void) { /* EMIOS 0 CH 0: Modulus Up Counter */
EMIOS_0.CH[0].CADR.R =16 ; /* Period will be 16/0,4MHz = 40 us or 25 kHz */
EMIOS_0.CH[0].CCR.B.MODE = 0x50; /* Modulus Counter Buffered (MCB) */
EMIOS_0.CH[0].CCR.B.BSL = 0x03; /* Use internal counter */
EMIOS_0.CH[0].CCR.B.UCPRE=0; /* Set channel prescaler to divide by 1 */
EMIOS_0.CH[0].CCR.B.UCPEN = 1; /* Enable prescaler; uses default divide by 1*/
EMIOS_0.CH[0].CCR.B.FREN = 1; /* Freeze channel counting when in debug mode*/
}

//Divisores de frecuencia para la camara SI

void initEMIOS_0ch8(void) { /* EMIOS 0 CH 23: Modulus Up Counter */
EMIOS_0.CH[8].CADR.R = 2400; /* Period will be  8000/0,4MHz= 20mS  or 50 Hz  */
EMIOS_0.CH[8].CCR.B.MODE = 0x50; /* Modulus Counter Buffered (MCB) */
EMIOS_0.CH[8].CCR.B.BSL = 0x03; /* Use internal counter */
EMIOS_0.CH[8].CCR.B.UCPRE=0; /* Set channel prescaler to divide by 1 */
EMIOS_0.CH[8].CCR.B.UCPEN = 1; /* Enable prescaler; uses default divide by 1*/
EMIOS_0.CH[8].CCR.B.FREN = 1; /* Freeze channel counting when in debug mode*/
}

/*se cambiara de EMIOS_0.CH[8].CADR.R = 8000 ;  Period will be  8000/0,4MHz= 20mS  or 50 Hz  
at 3mS frecuency de 333.3Hz
*/

//Divisor para frecuencia de motro y servo
//servo
void initEMIOS_0ch23(void) { /* EMIOS 0 CH 23: Modulus Up Counter */
EMIOS_0.CH[23].CADR.R = 19999; /* Period will be 19999+1 = 2000 clocks /0,4 MHz= 5mS= 200 Hz*/							
EMIOS_0.CH[23].CCR.B.MODE = 0x50; /* Modulus Counter Buffered (MCB) */
EMIOS_0.CH[23].CCR.B.BSL = 0x03; /* Use internal counter */
EMIOS_0.CH[23].CCR.B.UCPRE=0; /* Set channel prescaler to divide by 1 */
EMIOS_0.CH[23].CCR.B.UCPEN = 1; /* Enable prescaler; uses default divide by 1*/
EMIOS_0.CH[23].CCR.B.FREN = 1; /* Freeze channel counting when in debug mode*/
}
//motores
void initEMIOS_0ch16(void) { /* EMIOS 0 CH 23: Modulus Up Counter */
EMIOS_0.CH[16].CADR.R = 399; /* Period will be 399+1 = 400 clocks /0,4 MHz= 1mS = 1 kHz*/
EMIOS_0.CH[16].CCR.B.MODE = 0x50; /* Modulus Counter Buffered (MCB) */
EMIOS_0.CH[16].CCR.B.BSL = 1; /* Use internal counter */
EMIOS_0.CH[16].CCR.B.UCPRE=0; /* Set channel prescaler to divide by 1 */
EMIOS_0.CH[16].CCR.B.UCPEN = 1; /* Enable prescaler; uses default divide by 1*/
EMIOS_0.CH[16].CCR.B.FREN = 1; /* Freeze channel counting when in debug mode*/


}




/**********************************************************************************
***********************************************************************************
**********************************************************************************/

//asignar salidas al canal del emios


/*Para camara*/
//output CK
/*Equivalencia de unidades para el ancho de pulso 1= 2,5uS,600 = 1,5 mS
*/
void initEMIOS_0ch7(void) { /* EMIOS 0 CH 22: Output Pulse Width Modulation*/
EMIOS_0.CH[7].CADR.R = 4; /* Leading edge when time is 10 uS*/
EMIOS_0.CH[7].CBDR.R = 12; /* Trailing edge when time is 30 uS*/
EMIOS_0.CH[7].CCR.B.BSL = 1; /* Use counter bus B */
EMIOS_0.CH[7].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output*/
EMIOS_0.CH[7].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
SIU.PCR[7].R = 0x0700; /* MPC56xxS: Assign EMIOS_0 ch 22 to pad PA7 */
} 

/*
void vfnInit_Emios_0_Mcb(uint8_t u8Channel, uint16_t u16Period)
{
  	EMIOS_0.CH[u8Channel].CADR.R      = u16Period;	     //Period will be u16Period clocks (usec) 
  	EMIOS_0.CH[u8Channel].CCR.B.MODE  = 0x50; 			 //Set as Modulus Up Counter Buffered (MCB) 
 	EMIOS_0.CH[u8Channel].CCR.B.UCPRE = 3;    			 //Set channel prescaler divide by 3+1=4 (to 1Mhz) 
  	EMIOS_0.CH[u8Channel].CCR.B.UCPEN = 1;    			 //Enable prescaler
  	EMIOS_0.CH[u8Channel].CCR.B.FREN  = 1; 				 //Freeze channel counting when in debug mode 
}*/



//output SI
void initEMIOS_0ch9(void) { /* EMIOS 0 CH 22: Output Pulse Width Modulation*/
EMIOS_0.CH[9].CADR.R = 0;  /* Leading edge when time is 0 uS*/
EMIOS_0.CH[9].CBDR.R = 8; /* Trailing edge when time is 20 uS*/
EMIOS_0.CH[9].CCR.B.BSL = 1; /* Use counter bus B */
EMIOS_0.CH[9].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output*/
EMIOS_0.CH[9].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
SIU.PCR[9].R = 0x0700; /* MPC56xxS: Assign EMIOS_0 ch 22 to pad PA9 */

}

//por iniciar la convercion de los datos con el ADCSIU.PCR[24].R = 0x2000;         /* MPC56xxB: Initialize PB[8] as ANS0 */
  
/*

	SIU.PCR[24].R = 0x2000;         // MPC56xxB: Initialize PB[8] as ANS0 
  	SIU.PCR[25].R = 0x2000;         // MPC56xxB: Initialize PB[9] as ANS1 
  	SIU.PCR[26].R = 0x2000;         // MPC56xxB: Initialize PB[10] as ANS2 

  	ADC.MCR.R = 0x20000000;         // Initialize ADC0 for scan mode 
  	ADC.NCMR[1].R = 0x00000007;     // Select ANS0:2 inputs for conversion 
  	ADC.CTR[1].R = 0x00008606;      // Conversion times for 32MHz ADClock 
  	ADC.MCR.B.NSTART=1;             // Trigger normal conversions for ADC0 
*/


/*
void vfninit_Pit(void) {
  PIT.PITMCR.R = 1;       // Enable PIT and configure timers to stop in debug mode 
  PIT.CH[1].LDVAL.R = 0x99999999;       // PIT1 timeout = 640000000 sysclks x 1sec/64M sysclks = 1 sec 
  PIT.CH[1].TCTRL.R = 0x000000001; // Enable PIT1 interrupt and make PIT active to count 
  
}*/




/*Para los motores de tracion*/
void initEMIOS_0ch20(void) { /* EMIOS 0 CH 22: Output Pulse Width Modulation*/
EMIOS_0.CH[20].CADR.R = 0; /* Leading edge when time is 0 uS*/
EMIOS_0.CH[20].CBDR.R = velocity; /* Trailing edge when time is value "velocity" */
EMIOS_0.CH[20].CCR.B.BSL = 1; /* Use counter bus B */
EMIOS_0.CH[20].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output*/
EMIOS_0.CH[20].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
SIU.PCR[68].R = 0x0700; /* MPC56xxS: Assign EMIOS_0 ch 22 to pad PE4 */
} 


void initEMIOS_0ch22(void) { /* EMIOS 0 CH 22: Output Pulse Width Modulation*/
EMIOS_0.CH[22].CADR.R = 0; /* Leading edge when channel counter bus=500*/
EMIOS_0.CH[22].CBDR.R = velocity; /* Trailing edge when channel's counter bus=999*/
EMIOS_0.CH[22].CCR.B.BSL = 1; /* Use counter bus B */
EMIOS_0.CH[22].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output*/
EMIOS_0.CH[22].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
SIU.PCR[70].R = 0x0700; /* MPC56xxS: Assign EMIOS_0 ch 22 to pad PE6 */
}


/*Para servo motor*/
void initEMIOS_0ch21(void) { /* EMIOS 0 CH 21: Output Pulse Width Modulation*/
EMIOS_0.CH[21].CADR.R = 0; /* Leading edge when channel counter bus=250*/
EMIOS_0.CH[21].CBDR.R = direction; /* Trailing edge when channel counter bus=1000us*/
EMIOS_0.CH[21].CCR.B.BSL = 0x0; /* Use counter bus A (default) */
EMIOS_0.CH[21].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output */
EMIOS_0.CH[21].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
SIU.PCR[69].R = 0x0700; /* MPC56xxS: Assign EMIOS_0 ch 21 to pad PE5 */
}








/*delay*/
int x=0;
void delay(){
	
	for(x=0;x<1000000;x++){ 
	
	}
	
}

void setupVelocity(void){

		if(SIU.GPDI[64].R==0 && velocity != 350){
			
				velocity +=15;
				delay();		
				
			}
		if(SIU.GPDI[65].R==0 && velocity != 40){
			
				velocity -=15;
				delay();		
				
			}
			
			
}

	


void setupDirection(void){

		if(SIU.GPDI[66].R==0 && direction != 720 ){
			
				direction+=5;
				delay();		
				
			}
	
		if(SIU.GPDI[67].R==0 && direction != 480){
			
				direction-=5;
				delay();		
				
			}
	
}

 
 
void main (void) {	
  volatile uint32_t i = 0; /* Dummy idle counter */

 
  initModesAndClock(); /* Initialize mode entries and system clock */
  initPeriClkGen();  /* Initialize peripheral clock generation for DSPIs */
  disableWatchdog(); /* Disable watchdog */
  initEMIOS_0();     /* Initialize eMIOS 0 to provide 1 MHz clock to channels */
  initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
  
  initEMIOS_0ch8(); /* Initialize eMIOS 0 channel 0 as OPWM, using ch 23 as time base */
  initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, using ch 23 as time base */
  initEMIOS_0ch9();
  
   initEMIOS_0ch23();
  initEMIOS_0ch16(); 
  
  
  initEMIOS_0ch20();
  initEMIOS_0ch22(); /* Initialize eMIOS 0 channel 0 as OPWM, using ch 23 as time base */
  initEMIOS_0ch21(); /* Initialize eMIOS 0 channel 1 as OPWM, using ch 23 as time base */
  
  
  initCONFIG_output();
  
  
  
  while (1) {
 // 	setupDirection();
  	setupVelocity();
  	//vfninit_Pit();
     }  /* Wait forever */
}